/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
#include "Machine.hpp"
#include <GL/gl.h>
#include <GL/glut.h>
#include "picking/glutils.hpp"
//#include "Core.hpp"
#include "Device.hpp"

using namespace MuRo;

/*
BEGIN_EVENT_TABLE(CMachine,wxEvtHandler)
  EVT_MENU(wxID_ANY, CMachine::OnMenuEvent)
  EVT_MENU(wxID_HIGHEST+1233, CMachine::OnMenuEvent)
END_EVENT_TABLE()*/
gim::CSync foosync; ///< using CSync as a class =)


CMachine::CMachine(std::string name, MuRoMainFrame *MuRo)
:MuRo(MuRo),
selected(false),
operationMode(KOperModeObservation),
 iPreviousOperationMode(KOperModeObservation),
status(KStatusIdle),
name(name),
referencePosition(NULL),
speedCtrl(NULL),
coordinateDrive(NULL)
{
  SetEvtHandlerEnabled(true);
  menu.SetEventHandler(this);
  wxMenuItem *item=new wxMenuItem(&menu, wxID_ANY, name);
  menu.Append(item);
  /*this->Connect(item->GetId(),wxEVT_COMMAND_MENU_SELECTED,
                wxCommandEventHandler( CMachine::OnMenuEvent ), NULL, this );*/
  //menu.Append(wxID_ANY,name);
  //menu.Append(wxID_HIGHEST+1233,"item");
  /*
  this->Connect(menu.Append(wxID_ANY,name)->GetId(),wxEVT_COMMAND_MENU_SELECTED,
                wxCommandEventHandler( CMachine::OnMenuEvent ) );
  this->Connect(menu.Append(wxID_HIGHEST+1233,"item")->GetId(),wxEVT_COMMAND_MENU_SELECTED,
                wxCommandEventHandler( CMachine::OnMenuEvent ) );
  this->Connect(menu.Append(wxID_ANY,"another item")->GetId(),wxEVT_COMMAND_MENU_SELECTED,
                wxCommandEventHandler( CMachine::OnMenuEvent ) );
  */
  LockDeviceMutex();
  devices.push_back(&positions);
  devices.push_back(&rangings);
  devices.push_back(&alarms);
  devices.push_back(&energys);
  devices.push_back(&maps);
  devices.push_back(&coordDrives);
  devices.push_back(&speedCtrls);
  UnlockDeviceMutex();
}

void CMachine::OnMenuEvent(wxCommandEvent &e)
{
  dPrint(1,"dfflsdfkldfkldfldf");
  dPrint(1,"menuitem %s selected", e.GetString().c_str())
}

// void CMachine::OnAlarmDataEvent (const MaCI::Alarm::CAlarmData &aData, const unsigned int seq,
//                                  MaCI::Alarm::CAlarmClient &aAlarmClient)
// {
//   
// }

// void CMachine::OnArrivedToPointEvent(const MaCI::CoordinateDrive::CCoordinateDriveData &aData, 
//                                      const unsigned int aDataSequence,
//           MaCI::CoordinateDrive::CCoordinateDriveClient &aCoordinateDriveClient)
// {
//   
// }
// 
// void CMachine::OnCoordinateEvent(const MaCI::CoordinateDrive::CCoordinateDriveData &aData, 
//                         const unsigned int aDataSequence,
//           MaCI::CoordinateDrive::CCoordinateDriveClient &aCoordinateDriveClient)
// {
//   
// }

void CMachine::AddPositionClient(MaCI::Position::CPositionClient* positionClient)
{
  dPrint(0, "Muro adding position client!");
  CPositionDevice *d=new CPositionDevice(positionClient,this);
  d->enabled=false;
  if (!HasPosition())
  {
    SetReferencePosition(d);
    d->enabled=true;
    
  }
  positions.push_back(d);
}

void CMachine::SetReferencePosition(CPositionDevice *position)
{
  referencePosition=position;
  deviceVector::iterator i;
  for (i=rangings.begin(); i!=rangings.end(); i++)
  {
    ((CRangingDevice*)*i)->SetPositionDevice(referencePosition);
  }
}

void CMachine::SetSpeedCtrlDevice(CSpeedCtrlDevice *speedCtrl)
{
  this->speedCtrl=speedCtrl;
}

void CMachine::SetCoordinateDriveDevice(CCoordinateDriveDevice *coordDrive)
{
  coordinateDrive=coordDrive;
}

CSpeedCtrlDevice* CMachine::GetSpeedCtrlDevice()
{
  return speedCtrl;
}

CCoordinateDriveDevice* CMachine::GetCoordinateDriveDevice()
{
  return coordinateDrive;
}

CPositionDevice* CMachine::GetReferencePosition()
{
  return referencePosition;
}

void CMachine::AddRangingClient(MaCI::Ranging::CRangingClient* rangingClient)
{
  CRangingDevice *d=new CRangingDevice(rangingClient,this);
  if (HasPosition())
    d->SetPositionDevice(referencePosition);
  
  d->GetDeviceInformation(2000);
  rangings.push_back(d);
  
}

void CMachine::AddAlarmClient(MaCI::Alarm::CAlarmClient* alarmClient)
{
  CAlarmDevice *d=new CAlarmDevice(alarmClient,this);
  alarms.push_back(d);
  d->SetPickingContext(MuRo->GetGLDisplay());
}

void CMachine::AddMapClient(MaCI::Map::CMapClient* mapClient)
{
  CMapDevice *d=new CMapDevice(mapClient,this);
  maps.push_back(d);
  
}

void CMachine::AddEnergyClient(MaCI::Energy::CEnergyClient* energyClient)
{
  CEnergyDevice *d=new CEnergyDevice(energyClient,this);
  energys.push_back(d);
  
}


void CMachine::AddSpeedCtrlClient(MaCI::SpeedCtrl::CSpeedCtrlClient* speedCtrlClient)
{
  CSpeedCtrlDevice *d=new CSpeedCtrlDevice(speedCtrlClient,this);
  if (!HasManualDrive())
    speedCtrl=d;
  speedCtrls.push_back(d);
  
}

void CMachine::AddCoordinateDriveClient(MaCI::CoordinateDrive::CCoordinateDriveClient* coordDriveClient)
{
  CCoordinateDriveDevice *d=new CCoordinateDriveDevice(coordDriveClient,this);
  coordDrives.push_back(d);
  coordinateDrive=d;
  d->SetPickingContext(MuRo->GetGLDisplay());
}

void CMachine::Select(bool selected)
{
  this->selected=selected;
}

bool CMachine::IsSelected() const
{
  return selected;
}

std::string CMachine::GetName() const
{
  return name;
}

void CMachine::SetViewParams(const TViewParams &view)
{
  this->view=view;
}

void CMachine::Draw()
{
  drawDevices();

}

void CMachine::DrawPickable()
{
  LockDeviceMutex();
  if (HasPosition())
    referencePosition->DrawPickable(MuRo->GetGLDisplay()->GetViewParams());
  UnlockDeviceMutex();
}


void CMachine::DrawGhost(float x, float y, float heading)
{
  //dPrint(0, "Drawing ghost");
  glMatrixMode(GL_MODELVIEW);
  glPushMatrix();
  glLoadIdentity();
  glPolygonMode(GL_FRONT_AND_BACK, GL_LINE);
  glTranslatef(x, y, 0);
  glRotatef(heading*(180.0/M_PI),0,0,1);
  glColor3f(0.8,0.8,0.8);
  referencePosition->drawIcon(MuRo->GetGLDisplay()->GetViewParams());
  glPopMatrix();

  LockDeviceMutex();
  deviceVector::iterator ii;
  for (ii=rangings.begin(); ii!=rangings.end(); ii++)
  {
    ((CRangingDevice*)(*ii))->rangingDisplay->Draw(x,y,heading);
  }
  UnlockDeviceMutex();
  
}

void CMachine::drawDevices()
{
  LockDeviceMutex();
  TViewParams tmp = MuRo->GetGLDisplay()->GetViewParams(); ///< this line added in debug

  //TODO: mutex for devices?
  deviceTree::iterator i;
    
  for (i=devices.begin(); i!=devices.end(); i++)
  {
    deviceVector::iterator ii;
    for (ii=(*i)->begin(); ii!=(*i)->end(); ii++)
    {
      //(*ii)->Draw(MuRo->GetGLDisplay()->GetViewParams());

      // The following line causes segfaults randomly. It could be caused by accessing devices by MuroMainframe
      // at the same time.
      (*ii)->Draw(tmp);
    }
  }
  UnlockDeviceMutex();
}

wxMenu* CMachine::GetMenu()
{
  return &menu;
}

deviceTree& CMachine::GetDeviceTree()
{
  return devices;
}

//TODO: how about acceleration?
void CMachine::Drive(float power, float steer)
{
  if (HasManualDrive())
  {
    float p,s;
    if (power > 0) {
      p=fabs(power)*speedCtrl->maxSpeed.speed;
    } else {
      p=fabs(power)*speedCtrl->minSpeed.speed;
    }
    if (steer > 0) {
      s=fabs(steer)*speedCtrl->maxSpeed.angularSpeed;
    } else {
      s=fabs(steer)*speedCtrl->minSpeed.angularSpeed;
    }
    
    /**
    ((MaCI::SpeedCtrl::CSpeedCtrlClient*)(speedCtrl->MaCIClient))->SetSpeed(p, s,
                                         speedCtrl->maxSpeed.acceleration);
    */

    // We command the speed according to the scale values in settingsFrame
    ((MaCI::SpeedCtrl::CSpeedCtrlClient*)(speedCtrl->MaCIClient))->SetSpeed(p * MuRo->GetSettingsFrame().GetSpeedScale(),
                                                                            s * MuRo->GetSettingsFrame().GetAngularSpeedScale(),
                                                                            speedCtrl->maxSpeed.acceleration);
    dPrint(1,"commanding with speed %f, angular speed %f, acceleration %f",p * MuRo->GetSettingsFrame().GetSpeedScale(),
           s * MuRo->GetSettingsFrame().GetAngularSpeedScale(),
           speedCtrl->maxSpeed.acceleration);

  }
}

void CMachine::DriveToPoint(float x, float y)
{
  /*
  dPrint(1,"sdgfhtfrewrewqa");
  if (HasCoordDrive())
  {
    dPrint(1,"drive to point %f %f",x,y);
    //coordDriveClient->SetDriveMode();
    
    if (coordDriveClient->SetCoordinate(x,y))
      dPrint(1,"Set coordinate");
    if (coordDriveClient->SetStart())
      dPrint(1,"started");
  }*/
}

void CMachine::AddPathPoint(float x, float y)
{
  if (HasCoordDrive())
  {
    coordinateDrive->AddPoint(x,y);
  }
}

void CMachine::RemoveSelectedPathPoints()
{
  if (HasCoordDrive())
  {
    coordinateDrive->RemoveSelectedPoints();
  }
}

void CMachine::StartDrivingPath()
{
  if (HasCoordDrive())
  {
    coordinateDrive->StartDriving();
  }
}

void CMachine::StopDriving()
{
  
}

bool CMachine::HasManualDrive()
{
  return speedCtrl!=NULL;
}

bool CMachine::HasCoordDrive()
{
  return coordinateDrive!=NULL;//coordDriveClient!=NULL;
}

bool CMachine::HasPosition()
{
  return referencePosition!=NULL;
}

bool CMachine::SetPose(float x, float y, float heading)
{
  if (HasPosition())
  {
    return ((MaCI::Position::CPositionClient*)referencePosition->MaCIClient)->SetPosition(
                                            MaCI::Position::TPose2D(x,y,heading));
  }
  return false;
}

float CMachine::GetPositionX()
{
  if (HasPosition())
  {
    return referencePosition->positionX;
  }
  return 0;
}

float CMachine::GetPositionY()
{
  if (HasPosition())
  {
    return referencePosition->positionY;
  }
  return 0;
}

float CMachine::GetHeading()
{
  if (HasPosition())
  {
    return referencePosition->heading;
  }
  return 0;
}

void CMachine::SetOperationMode(EOperationMode mode)
{
  switch (mode)
  {
    case KOperModeSetPosition:
      if (HasPosition() && (operationMode != KOperModeSetPosition))
      {
        iPreviousOperationMode = operationMode;
        operationMode=KOperModeSetPosition;
      } else if(HasPosition() && (operationMode == KOperModeSetPosition))
        {
          // We set the mode back to the previous mode.
          // This enables toggling between the mode
          operationMode = iPreviousOperationMode;

        }
      break;
    case KOperModeCoordDrive:
      if (HasCoordDrive())
      {
        operationMode=KOperModeCoordDrive;
      }
      break;
    case KOperModeManualDrive:
      if (HasManualDrive())
      {
        operationMode=KOperModeManualDrive;
      }
      break;
    default:
      operationMode=KOperModeObservation;
      break;
  }
  dPrint(1,"Machines operation mode set to %d", operationMode)
}

void CMachine::SetPositionHistoryLengthS(int length)
{
  if (HasPosition())
  {
    referencePosition->historyLifetimeS=length;
  }
}

void CMachine::ShowPositionHistory(bool history)
{
  if (HasPosition())
  {
    referencePosition->showHistory=history;
  }
}

bool CMachine::IsPositionHistoryShown()
{
  if (HasPosition())
  {
    return referencePosition->showHistory;
  }
  return false;
}

void CMachine::SetRangingHistoryLengthS(int length)
{
  deviceVector::iterator ii;
  for (ii=rangings.begin(); ii!=rangings.end(); ii++)
  {
    ((CRangingDevice*)(*ii))->rangingDisplay->SetHistoryLengthMs(length*1000);
  }
}

void CMachine::ShowRangingHistory(bool history)
{
  deviceVector::iterator ii;
  for (ii=rangings.begin(); ii!=rangings.end(); ii++)
  {
    ((CRangingDevice*)(*ii))->rangingDisplay->EnableHistory(history);
  }
}

bool CMachine::IsRangingHistoryShown()
{
  if (rangings.size())
    return ((CRangingDevice*)(*rangings.begin()))->rangingDisplay->IsHistoryEnabled();


  dPrint(0, "Returning false here, might be wrong?");
  return false;

}


// Machine device mutex functions
void CMachine::LockDeviceMutex()
{
  foosync.Lock();
}

void CMachine::UnlockDeviceMutex()
{
  foosync.Unlock();

}
